#include <ros/ros.h>
#include <geometry_msgs/QuaternionStamped.h>
#include <nav_msgs/Odometry.h>
#include "transform_utils/geometry_transform.h"
#include "transform_utils/type_transform.h"
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>

using namespace Planner;
using Eigen::Vector3d;
using Eigen::Quaterniond;

ros::Publisher dji_orien_pub, transformed_orien_pub;

void djiOrienCallback(const geometry_msgs::QuaternionStamped &msg)
{
    Quaterniond orien;
    Planner::geometry2EigenQuaternion(msg.quaternion, orien);
    Vector3d ypr = quaternion_to_ypr(orien);
    // ROS_INFO("yaw: %f, pitch: %f, roll: %f", ypr[0], ypr[1], ypr[2]);
    
    Vector3d transformed_ypr;
    transformed_ypr[0] = (M_PI / 2 - ypr[0]);
    transformed_ypr[1] = -ypr[1];
    transformed_ypr[2] = ypr[2];
    Quaterniond transformed_orien = ypr_to_quaternion(transformed_ypr);
    ROS_INFO("yaw: %f, pitch: %f, roll: %f", transformed_ypr[0], transformed_ypr[1], transformed_ypr[2]);
    
    nav_msgs::Odometry pose_msg;
    pose_msg.header.frame_id = "map";
    pose_msg.pose.pose.orientation = msg.quaternion;
    pose_msg.pose.pose.position.x = 1;
    pose_msg.pose.pose.position.y = -1;
    dji_orien_pub.publish(pose_msg);

    pose_msg.pose.pose.position.y = 1;
    pose_msg.pose.pose.orientation.x = transformed_orien.x();
    pose_msg.pose.pose.orientation.y = transformed_orien.y();
    pose_msg.pose.pose.orientation.z = transformed_orien.z();
    pose_msg.pose.pose.orientation.w = transformed_orien.w();
    transformed_orien_pub.publish(pose_msg); // 转换后为FLU坐标系
}

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "dji_controller");
    ros::NodeHandle nh;

    ros::Subscriber dji_orien_sub_ = nh.subscribe("/dji_osdk_ros/attitude", 1,
                                                 djiOrienCallback);
    dji_orien_pub = nh.advertise<nav_msgs::Odometry>("/dji_origin_orien", 1);
    transformed_orien_pub = nh.advertise<nav_msgs::Odometry>("/transformed_orien", 1);

    ros::Rate rate(100);
    while (ros::ok())
    {
        ros::spinOnce();
        rate.sleep();
    }
    
    return 0;
}
